﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using JamesZhao.Engine.GameObjects;
using JamesZhao.Engine.Graphics.RenderStages;
using JamesZhao.Engine.Utils;
using SlimMath;

namespace JamesZhao.Engine.GameComponents
{
    public class SteerComponent : GameComponent
    {
        private const float B = 3.25f;
        private const float K_G = 7.5f;
        private const float C1 = .4f;
        private const float C2 = .01f;
        private SteeringVisualizer _steeringVisualizer;
        public SteerComponent(GameObject owner)
            : base(owner)
        {
            _steeringVisualizer = new SteeringVisualizer();
            _steeringVisualizer.Show();
        }

        public ReadGpuValueStage ReadGpuValueStage { get; set; }
        public Vector3 Target { get; set; }
        private Quaternion _previousOrientation = Quaternion.Zero;
        public override void Update()
        {
            var obstacleRotation = Quaternion.RotationYawPitchRoll(ReadGpuValueStage.Result.X / 10000,
                                                                       ReadGpuValueStage.Result.Y / 10000, 0);

            _steeringVisualizer.Force = ReadGpuValueStage.Result;
            var goalRotation = Quaternion.Identity;
            //if (_previousOrientation != Quaternion.Zero)
            {
                var angVel = Transform.LocalRotation - _previousOrientation;
                var targetDir = (Target - Transform.LocalPosition);
                var from = Vector3.Transform(Vector3.UnitZ, Transform.LocalRotation);
                var to = targetDir;
                var dh = Quaternion.FromTo(from, to);
                var goalDistance = (Target - Transform.LocalPosition).Length();


                var angularAcceleration =
                    //-B*angVel
                    //- K_G*dh*(float) (Math.Exp(-C1*goalDistance) + C2);
                    Quaternion.Lerp(Quaternion.Identity, dh, (float) (Math.Exp(-C1 * goalDistance) + C2));
                
                goalRotation = angularAcceleration;

            }
            Transform.LocalRotation = goalRotation * Transform.LocalRotation;
            Transform.LocalRotation = obstacleRotation * Transform.LocalRotation;
            _previousOrientation = Transform.LocalRotation;

        }
    }
}
